#include <iostream>
#include <stdio.h>
#include "stdlib.h"
#include <math.h>
#include "navigator.h"
#include "field.h"
#include "node.h"
#include "time.h"
#include <windows.h>

using namespace std;
using namespace nav;
//!TODO Test with ArmCross Compile

namespace nav
{
#define DEBUG true
#define SIZEX 32
#define SIZEY 32
#define XMAXDIGIT 2
#define YMAXDIGIT 2


#define OBSTICLE 65533
#define UNEXPLORED 0
}

//struct coord{int x; int y}

bool isNumber(char c)
{
    return (int)c>=(int) '0' && (int)c<=(int)'9';
}

int main(int argc, char *argv[])
{
    //Initialisation
     srand(time(NULL));
    int xArgh[XMAXDIGIT];
    int xIndex=0;
    int yIndex=0;
    int yArgh[YMAXDIGIT];

    bool secondNumber     =false;
    bool secondNumberStart=false;
    int xCoordTarg, yCoordTarg;

    //Arguments handled fromt the outside world - Heresy. There is no such thing as a outside world.
    if(argc<=5)
    {
        xCoordTarg=rand()%SIZEX;
        yCoordTarg=rand()%SIZEY;
    }
    else   //silly, silly silly - get on with it.
    {
        for( int i = 0; i < argc; ++i )
        {
            if (*argv[i]!= ' ' && isNumber(*argv[i])==true && secondNumber==false)
            {
                yArgh[yIndex]=atoi(argv[i]);
                yIndex++;

            }

            if (*argv[i]==' ' && secondNumberStart==false)
            {
                secondNumber=true;
            }

            if (*argv[i]!= ' ' && isNumber(*argv[i])==true && secondNumber==false)
            {
                secondNumberStart=true;
                xArgh[xIndex]=atoi(argv[i]);
                xIndex++;
            }
        }


        xCoordTarg=0;
        for (int i=xIndex; i>= 0; i--)
            xCoordTarg=xCoordTarg+xArgh[i]*pow(10,i);
        yCoordTarg=0;
        for (int i=yIndex; i>= 0; i--)
            yCoordTarg=yCoordTarg+yArgh[i]*pow(10,i);


    }



    //Mainloop every target is handled
    //TODO define a container for a series of targets
    printf("Intialisation complete");

    while (true)
    {
        float LostAccuracy=0.0f;
        //declare the target Node
        Node* target = new Node(xCoordTarg,
                                yCoordTarg,
                                0,
                                true);
        //declar the nextNode, which is just a container for the next Node to go towards
        Node* NextNode= new Node(xCoordTarg,
                                yCoordTarg,
                                0,
                                true);

        //find a Navigator with random target
        Navigator navi(target->posX,target->posY,SIZEX/2, SIZEY/2,UP, SIZEX,SIZEY);

        //temp O Rarys Variables
            bool OperationSuccess=true;
            bool * bOperationPtr= &OperationSuccess;
            dirEncoded currDir=navi.dir;

        //Test function which generates a test map
            navi.Map.RandoMazeField(22,4,12,7,navi.PositionX,navi.PositionY,target->posX,target->posY);
        //go for it
          cout<<"Randomizing Field finnished"<<std::endl;
        while (navi.TargetReached()==false)
        {
            /*!Think*/
              cout<<"Think"<<std::endl;
            //Display the Map
            navi.Map.displayMap(navi.PositionX,navi.PositionY,navi.targetX,navi.targetY);
            //we use the target Algo
            NextNode=navi.goForIt(navi.PositionX,navi.PositionY,target->posX,target->posY);

                /*!Sense*/
                cout<<"Sense"<<endl;
                currDir=navi.dir;
                //TODO Decision wether the Unit needs to make a turn
                LostAccuracy=navi.doATurn(navi.Map.posToDir(navi.PositionX,navi.PositionY,target->posX,target->posY),bOperationPtr);

                //TODO Blend Lost Accuracy by Orthogonal Direction into Map
                navi.blendTurnIntoMap(currDir,navi.dir, LostAccuracy, OperationSuccess);

                //we found a obsticle -- we skip the movement and rethink the steps
                if ( navi.doSensorSweepInDir()==1) continue;

            cout<<"Act"<<endl;
            /*!Action*/
            //we make a Move
            Sleep(500);
            LostAccuracy=navi.makeAMove(NextNode,navi.dir,bOperationPtr);
            //we reset the DebugInfo
            navi.Map.resetDebugInfo();
            //!Handle Action
            //TODO Blend Lost Accuracy by Direction into Map
            navi.blendMoveIntoMap(currDir,LostAccuracy,OperationSuccess);

        }
                cout << "End of TestRun" << endl;

                Sleep(1000);


        }


    return 0;
}
